#include "car.h"

pid_t PID_xunji = {0};
car_speed_t car_speed = {0};

void car_init (void)
{
    car_pid_xunji_Init();
    car_set_base_speeds(200, 200);
}

void car_set_base_speeds(int tracker_base_speed, int rotate_base_speed)
{
    // 设置巡线和旋转的基础速度
    car_speed.tracker_base_speed = tracker_base_speed;
    car_speed.rotate_base_speed = rotate_base_speed;
}
void car_set_rotate_speed(int rotate_speed)
{
    // 设置旋转速度
    car_speed.rotate_base_speed = rotate_speed;
}
void car_set_tracker_speed(int tracker_speed)
{
    // 设置巡线速度
    car_speed.tracker_base_speed = tracker_speed;
}
void car_pid_xunji_Init(void)
{
    PID_xunji.Kp = 10;
    PID_xunji.Ki = 0;
    PID_xunji.Kd = 0.3;
    
    PID_xunji.PID_Limit_MAX = 999;
    PID_xunji.Ki_Limit_MAX = 50;
}
void car_track_line(uint8_t tracker_digital_sensors[8])
{
    // 具体的巡线逻辑
    int sum = 0;
    int count = 0;

    if (tracker_digital_sensors[0])
    {
        sum += -10;
        count++;
    }
    if (tracker_digital_sensors[1])
    {
        sum += -7;
        count++;
    }
    if (tracker_digital_sensors[2])
    {
        sum += -4;
        count++;
    }
    if (tracker_digital_sensors[3])
    {
        sum += -1;
        count++;
    }
    if (tracker_digital_sensors[4])
    {
        sum += 1;
        count++;
    }
    if (tracker_digital_sensors[5])
    {
        sum += 4;
        count++;
    }
    if (tracker_digital_sensors[6])
    {
        sum += 7;
        count++;
    }
    if (tracker_digital_sensors[7])
    {
        sum += 10;
        count++;
    }

    if (count == 0)
    {
        // 丢线处理
        set_sys_running_state(0); // 切换到IDLE状态

    }
    else
    {
        PID_xunji.Error[0] = (float)sum / count;
    }


    PID_Calculate(&PID_xunji, PID_xunji.Error[0], 0);
    //串口发送左右轮速度
    char msg[50] = {0};
    sprintf(msg, "L:%03d R:%03d", (int)(car_speed.tracker_base_speed - PID_xunji.PID_Out), (int)(car_speed.tracker_base_speed + PID_xunji.PID_Out));
    // Uart_Debug_transmitString(msg);
    OLED_ShowString(0, 6, (uint8_t*)msg, 8);
    Set_Motor_PWM((int)(car_speed.tracker_base_speed - PID_xunji.PID_Out), (int)(car_speed.tracker_base_speed + PID_xunji.PID_Out));
}

void car_rotate(float angle)
{
    // 具体的旋转逻辑
}
